#include <Arduino.h>
#include <Wire.h>
#include "BluetoothSerial.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "plane_header.h"

// float P_roll = 200;
// float I_roll = 0;
// float D_roll = 800;

// float P_pitch = 200;
// float I_pitch = 0;
// float D_pitch = 800;

// float P_yaw = 400;
// float I_yaw = 0;
// float D_yaw = 800;
const int ledPin = 21;
const int AD_pin = 35;
const int LED1_pin = 20;
const int LED2_pin = 7;
const int LED3_pin = 8;
const int LED4_pin = 5;
const int MPU_INT_pin = 34;

const int PWM1_pin = 22;
const int PWM2_pin = 2;
const int PWM3_pin = 21;
const int PWM4_pin = 19;

const int sda_pin = 33;
const int scl_pin = 32;

const int freq = 5000;//PWM频率
const int resolution = 8;//PWM分辨率
BluetoothSerial SerialBT;
// 角度环
float P_roll  = 60;
float I_roll  = 0;
float D_roll  = 10;//15

float P_pitch = 60;
float I_pitch = 0;
float D_pitch = 10;//15

float P_yaw   = 0;
float I_yaw   = 0;
float D_yaw   = 0;//16

// 角速度环
float P_roll_w = 20;
float I_roll_w = 0;
float D_roll_w = 15;

float P_pitch_w = 20;
float I_pitch_w = 0;
float D_pitch_w = 15;

float P_yaw_w = 32;
float I_yaw_w = 0;
float D_yaw_w = 16;

float agx = 0, agy = 0, agz = 0; //角度变量

// 全局迭代变量
int counter = 0;
float y_log[INTERVAL], p_log[INTERVAL], r_log[INTERVAL];

int signal_PWM_1 = 10;//电机控制信号，取值范围0~255
int signal_PWM_2 = 20;
int signal_PWM_3 = 30;
int signal_PWM_4 = 40;

float signal_first_PWM_1 = 160;//电机初始控制信号，取值范围0~255，220起飞临界
float signal_first_PWM_2 = 160;
float signal_first_PWM_3 = 160;
float signal_first_PWM_4 = 160;

MPU6050 mpu1;
uint8_t fifoBuffer[128];
Quaternion q;
VectorFloat gravity;
float ypr[3];
float pitch_zero = 0.10; // 6degree zero shift
float roll_zero  = 0;
float ypr_target[3] = {0, pitch_zero, roll_zero};
int16_t gyro_int[3];
// 单独的全局变量，未在h文件内声明，仅仅用作测试
bool IS_PRINT = false;

void setup() {
  Serial.begin(115200);

  pinMode(AD_pin, INPUT);
  pinMode(MPU_INT_pin, INPUT);

  //  pinMode(LED1_pin, OUTPUT);
  //  pinMode(LED2_pin, OUTPUT);
  //  pinMode(LED3_pin, OUTPUT);
  //  pinMode(LED4_pin, OUTPUT);

  ledcAttach(PWM1_pin, freq, resolution);
  ledcAttach(PWM2_pin, freq, resolution);
  ledcAttach(PWM3_pin, freq, resolution);
  ledcAttach(PWM4_pin, freq, resolution);

  Wire.setPins(sda_pin, scl_pin);
  Wire.setClock(400000);
  Wire.begin();

  SerialBT.begin("ESP32_Drone_C");   //启动蓝牙串口并设置蓝牙的名称

  //******************************************************
  mpu1.initialize();
  mpu1.dmpInitialize();
  mpu1.CalibrateGyro(6);
  mpu1.PrintActiveOffsets();
  mpu1.setDMPEnabled(true);
  mpu1.setRate(1);

  // 用于寻找角速度
  // 绕x轴旋转的角度
  // float x_old = 0;
  // float dx;
  // unsigned long time_last = 0;
  // float omega_d = 0; 

  // int Start_Counter = 10;
  while (1)
  {
    if (mpu1.dmpGetCurrentFIFOPacket(fifoBuffer))
    {
      mpu1.dmpGetQuaternion(&q, fifoBuffer);
      mpu1.dmpGetGravity(&gravity, &q);
      mpu1.dmpGetYawPitchRoll(ypr, &q, &gravity);
      mpu1.dmpGetGyro(gyro_int,fifoBuffer);
    }

    SerialBT.print("Battery=");
    SerialBT.print(float(analogReadMilliVolts(AD_pin)) * 0.0476);
    SerialBT.println("\%");

    SerialBT.print("ypr\t");
    SerialBT.print(ypr[0] * 180 / PI);
    SerialBT.print("\t");
    SerialBT.print(ypr[1] * 180 / PI);
    SerialBT.print("\t");
    SerialBT.println(ypr[2] * 180 / PI);
    // print gyro(角速度？)
    // dx = ypr[0] * 180 / PI - x_old;
    // x_old = ypr[0] * 180 / PI;
    // omega_d = dx / (millis() - time_last) * 1000;
    // time_last = millis();
    // // 打印两种角速度
    // SerialBT.print("omega_d: \t");
    // SerialBT.println(omega_d);
    // SerialBT.print("omega_g:\t");
    // SerialBT.println(gyro_int[0] + 1);
    // SerialBT.print("dt: \t");
    // SerialBT.println(millis() - time_last);
    // time_last = millis();
    // 打印gyro
    SerialBT.print("gyro_int \t");
    SerialBT.print(gyro_int[0]);
    SerialBT.print("\t");
    SerialBT.print(gyro_int[1]);
    SerialBT.print("\t");
    SerialBT.println(gyro_int[2]);
    SerialBT.println("Enter 's' to start booting...");
    // SerialBT.println(millis());
    // delay(500);


    if (SerialBT.available())
    {
      char temp_char = SerialBT.read();
      if (temp_char == 's')
        break;
    }
  }//等待启动指令

  // Preheat();//预启动
}

void loop() {
  if (mpu1.dmpGetCurrentFIFOPacket(fifoBuffer))
  {
    // if (counter > INTERVAL - 600 && counter % 5 == 0 && counter < INTERVAL - 500)
    // {
    //   // signal_first_PWM_1--;
    //   // signal_first_PWM_2--;
    //   // signal_first_PWM_3--;
    //   // signal_first_PWM_4--;
    //   signal_first_PWM_1++;
    //   signal_first_PWM_2++;
    //   signal_first_PWM_3++;
    //   signal_first_PWM_4++;
    // }

    mpu1.dmpGetQuaternion(&q, fifoBuffer);
    mpu1.dmpGetGravity(&gravity, &q);
    mpu1.dmpGetYawPitchRoll(ypr, &q, &gravity);
    mpu1.dmpGetGyro(gyro_int,fifoBuffer);
    // 记录ypr
    // y_log[counter - 1] = ypr[0];
    // p_log[counter - 1] = ypr[1];
    // r_log[counter - 1] = ypr[2];
    //倾倒检测
    Emergency_Check(ypr);
    // 控制
    if(SerialBT.available()){
      String command = SerialBT.readString();
      ud_command(command);
      change_PID(command);
      if(command == "p"){
        IS_PRINT = !IS_PRINT;
      }
    }
    if(IS_PRINT){
      SerialBT.print(signal_PWM_1);
      SerialBT.print("\t");
      SerialBT.print(signal_PWM_2);
      SerialBT.print("\t");
      SerialBT.print(signal_PWM_3);
      SerialBT.print("\t");
      SerialBT.println(signal_PWM_4);
    }
    
    // PID参数计算
    calculate_pid(ypr, ypr_target, gyro_int);
    //修改电机转速
    // Motor_Drive(signal_PWM_1, signal_PWM_2, signal_PWM_3, signal_PWM_4);
    // counter 超过1000 不加了
    if(counter < 1000){
      counter++;
    }
  }
  else
  {
    // UART_process();
  }
}












